





#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>

#include "drv/drv_socket.h"
#include "drv/drv_leds.h"
#include "drv/drv_servo.h"
#include "drv/drv_timer.h"

#include "sensors/bmp085.h"
#include "sensors/hmc5883l.h"
#include "sensors/l3g4200.h"
#include "sensors/venus638.h"
#include "sensors/stlis331dlh.h"


void timer_callback( int sig ) ;
void term_callback(int sig);

void SocketHandler ( char *buf , int nb_bytes  )
{
        printf("%s \n", buf);
}    
   

double startTime, endTime;

int main(int argc, char *argv[])
{
        printf("**********************************************\n");
        //init des leds
        //InitLeds();

        //init des capteurs
        InitSensors();

        //init des moteurs
        //InitServos();

        //init socket
        //drv_socketOpen(7000, SocketHandler);
        
        //callback when closed
        signal(SIGINT,term_callback);

	//get time
        startTime = drv_timerGetTime();
        //boucle principale
        while(1)
        {
                double delta_time = drv_timerGetTime() - startTime;
                if( delta_time >= 500000000UL ) //10ms
                {
                        //printf("%.0f ns\n", delta_time);
                        SensorsRead( );
                        SensorsCompute(delta_time);
                        startTime = drv_timerGetTime();
                }
                usleep(50);
                        
        }
	//stop servo
	//DeInitServos();
	//Stop sensors
        DeInitSensors();
        //stop la liaison TCP
        //drv_socketClose ();
        return EXIT_SUCCESS;
}

//SIGTERM Callback
void term_callback(int sig)
{
	//stop servo
	//DeInitServos();
        //Stop sensors
        DeInitSensors();
        //stop la liaison TCP
        //drv_socketClose ();
        exit(1);
}



